/**
 * Copyright (C) 2025 pony working team
 * SPDX-License-Identifier: MulanPSL-2.0
 */

/* includes (local) ----------------------------------------------------------*/
#include <pony/sensor/mpu60x0.h>
#include <hal_i2c.h>
/* includes (standard library, system) ---------------------------------------*/
/* includes (other library) --------------------------------------------------*/
/* includes (project) --------------------------------------------------------*/
#include <x_config.h>
#include <x_module.h>

/* defines -------------------------------------------------------------------*/
/* typedefs ------------------------------------------------------------------*/

typedef struct mpu60x0 {
    hal_i2c_dev_t dev;
} mpu60x0_t;

/* macro ---------------------------------------------------------------------*/
/* variables (extern) --------------------------------------------------------*/
/* variables (local) ---------------------------------------------------------*/

static mpu60x0_t s_mpu60x0;

/* variables (global) --------------------------------------------------------*/
/* inlines -------------------------------------------------------------------*/
/* functions -----------------------------------------------------------------*/

static int __mpu60x0_write_reg(hal_i2c_dev_t *dev, uint8_t reg, uint8_t val)
{
    return hal_i2c_dev_write(dev, reg, &val, 1);
}

static int __mpu60x0_read_reg(hal_i2c_dev_t *dev, uint8_t reg, uint8_t *data, uint16_t size)
{
    return hal_i2c_dev_read(dev, reg, data, size);
}

static int __mpu60x0_init(hal_i2c_dev_t *dev)
{
    __mpu60x0_write_reg(dev, MPU60X0_REG_PWR_MGMT_1, 0x00);
    __mpu60x0_write_reg(dev, MPU60X0_REG_SMPLRT_DIV, 0x07);
    __mpu60x0_write_reg(dev, MPU60X0_REG_CONFIG, 0x06);
    __mpu60x0_write_reg(dev, MPU60X0_REG_ACCEL_CONFIG, 0x01);
    __mpu60x0_write_reg(dev, MPU60X0_REG_GYRO_CONFIG, 0x18);
    return 0;
}

uint8_t mpu60x0_read_id(void)
{
    uint8_t id = 0;
    __mpu60x0_read_reg(&s_mpu60x0.dev, MPU60X0_REG_WHO_AM_I, &id, 1);
    return id;
}

int mpu60x0_read_accel(mpu60x0_accel_t *accel)
{
    uint8_t data[6];
    int ret;

    ret = __mpu60x0_read_reg(&s_mpu60x0.dev, MPU60X0_ACCEL_OUT, data, 6);
    if (ret != 0) {
        return -1;
    }
    accel->x = (int16_t)((data[0] << 8) | data[1]);
    accel->y = (int16_t)((data[2] << 8) | data[3]);
    accel->z = (int16_t)((data[4] << 8) | data[5]);
    return 0;
}

int mpu60x0_read_gyro(mpu60x0_gyro_t *gyro)
{
    uint8_t data[6];
    int ret;

    ret = __mpu60x0_read_reg(&s_mpu60x0.dev, MPU60X0_GYRO_OUT, data, 6);
    if (ret != 0) {
        return -1;
    }
    gyro->x = (int16_t)((data[0] << 8) | data[1]);
    gyro->y = (int16_t)((data[2] << 8) | data[3]);
    gyro->z = (int16_t)((data[4] << 8) | data[5]);
    return 0;
}

int mpu60x0_read_temp(int16_t *temp)
{
    uint8_t data[2];
    int ret;

    ret = __mpu60x0_read_reg(&s_mpu60x0.dev, MPU60X0_TEMP_OUT, data, 2);
    if (ret != 0) {
        return -1;
    }
    // expand the temperature 100 times
    *temp = (int16_t)((data[0] << 8) | data[1]) * 100 / 340 + 3653;
    return 0;
}

static int __mpu60x0_open(hal_dev_t *dev, int oflag, va_list valist)
{
    hal_i2c_dev_t *i2c_dev = (hal_i2c_dev_t *)dev;

    return __mpu60x0_init(i2c_dev);
}

static hal_dev_ops_t dev_ops = {
    .open = __mpu60x0_open,
};

static int mpu60x0_init(void)
{
    int ret;

    s_mpu60x0.dev.masize = 1;
    ret = hal_i2c_dev_register(&s_mpu60x0.dev, CONFIG_MPU60X0_I2C_BUS, "mpu60x0",
                               CONFIG_MPU60X0_I2C_ADDR);
    if (ret == 0) {
        ret = hal_dev_init((hal_dev_t *)&s_mpu60x0, &dev_ops);
    }

    return ret;
}

MODULE_INIT(driver, mpu60x0_init);
